#include "ArduinoControllerDevice.h"

ArduinoControllerDevice::ArduinoControllerDevice(IODeviceContext* context)
	: IODeviceWithThreadSupport(IO_DEVICE_ARDUINO_CONTROLLER, context)
	, m_pSerial(NULL)
{

}


ArduinoControllerDevice::~ArduinoControllerDevice()
{
}

int ArduinoControllerDevice::Init()
{
	m_pSerial = new serial::Serial;
	m_pSerial->setPort("COM3");
	m_pSerial->setBaudrate(115200);
	//m_pSerial->setPort(m_Context->GetArduinoControllerPort());
	//m_pSerial->setBaudrate(m_Context->GetArduinoControllerBaudRate());
	m_pSerial->setTimeout(serial::Timeout::simpleTimeout(100));
	try {
		m_pSerial->open();
		if (!m_pSerial->isOpen()) {
			return -1;
		}
		SendOperation(IO_OPERATION_ARDUINO_CONTROLLER_GET_VALUE, 0);
	}
	catch (serial::IOException& e){
		return -1;
	}

	Start();
	return 1;
}

int ArduinoControllerDevice::DeInit()
{
	Stop();
	try {
		if (!m_pSerial->isOpen()) {
			return -1;
		}
		m_pSerial->close();
	}
	catch (serial::IOException& e){
		return -1;
	}
	delete m_pSerial;
	m_pSerial = NULL;
	return 1;
}

void ArduinoControllerDevice::OnOperation(IODeviceOperation operation, unsigned int param)
{
	if (operation == IO_OPERATION_ARDUINO_CONTROLLER_GET_VALUE) {
		ArduinoControllerData output;
		ReadData(m_Data);
		if (HandleButtonData(m_Data, output) == 0) {
			Notice(GetType(), IO_OPERATION_ARDUINO_CONTROLLER_GET_BUTTON_VALUE, reinterpret_cast<unsigned int>(&output));
			m_Data.clear();
		}
		else if (HandleRockerData(m_Data, output) == 0) {
			Notice(GetType(), IO_OPERATION_ARDUINO_CONTROLLER_GET_ROCKER_VALUE, reinterpret_cast<unsigned int>(&output));
			m_Data.clear();
		}
		SendOperation(operation, 0);
	}
}

void ArduinoControllerDevice::ReadData(BDDeque<uint8_t>& data)
{
	if (m_pSerial == NULL) {
		return;
	}

	if (!m_pSerial->isOpen()) {
		return;
	}

	uint8_t value = 0;
	try {
		m_pSerial->read(&value, 1);
		data.push_back(value);
	}
	catch (serial::IOException& e){

	}
	
}

int ArduinoControllerDevice::HandleButtonData(BDDeque<uint8_t>& data, ArduinoControllerData& output)
{
	while (data.size() > 0 && data[START_CODE_INDEX_1] != START_CODE) {
		data.pop_front();
	}

	if (data.size() >= BUTTON_FRAME_LENGTH && data[START_CODE_INDEX_1] == START_CODE && data[START_CODE_INDEX_2] == START_CODE && data[END_CODE_BUTTON_INDEX] == END_CODE) {
		output.buttonValue = data[BUTTON_INDEX];
		output.buttonCount = 3;
		return 0;
	}
	return -1;
}

int ArduinoControllerDevice::HandleRockerData(BDDeque<uint8_t>& data, ArduinoControllerData& output)
{
	while (data.size() > 0 && data[START_CODE_INDEX_1] != START_CODE) {
		data.pop_front();
	}

	if (data.size() >= ROCKER_FRAME_LENGTH && data[START_CODE_INDEX_1] == START_CODE && data[START_CODE_INDEX_2] == START_CODE && data[END_CODE_ROCKER_INDEX] == END_CODE) {
		output.x1 = data[ROCKER_X1_INDEX];
		output.y1 = data[ROCKER_Y1_INDEX];
		output.x2 = data[ROCKER_X2_INDEX];
		output.y2 = data[ROCKER_Y2_INDEX];
		printf("%d, %d, %d, %d\n", output.x1, output.y1, output.x2, output.y2);
		return 0;
	}
	return -1;
}